#include "GaborUtility.h"

int GaborUtility::k = 1;

double GaborUtility::gradientX(IplImage *img, int x, int y) {
    double sum = 0;
    for (int i = x - 1; i <= x + 1; i++)
        for (int j = y - 1; j <= y + 1; j++) {
            if (i < 0 || j < 0) continue;
            sum += img->imageData[img->widthStep * j + i] * (i - x);
        }

    sum *= 2.0 / 6.0;
    return sum;
}

double GaborUtility::gradientY(IplImage *img, int x, int y) {
    double sum = 0;
    for (int i = x - 1; i <= x + 1; i++)
        for (int j = y - 1; j <= y + 1; j++) {
            if (i < 0 || j < 0) continue;
            sum += img->imageData[img->widthStep * j + i] * (j - y);
        }

    sum *= 2.0 / 6.0;
    return sum;
}

double GaborUtility::gxy(IplImage *img, int x, int y) {
    double sum = 0;
    for (int p = x - (k - 1); p <= x + (k - 1); p++)
        for (int q = y - (k - 1); q <= y + (k - 1); q++) {
            sum += gradientX(img, p, q) * gradientY(img, p, q);
        }
    return sum;
}

double GaborUtility::gxx(IplImage *img, int x, int y) {
    double sum = 0;
    for (int p = x - (k - 1); p <= x + (k - 1); p++)
        for (int q = y - (k - 1); q <= y + (k - 1); q++) {
            sum += gradientX(img, p, q) * gradientX(img, p, q);
        }
    return sum;
}

double GaborUtility::gyy(IplImage *img, int x, int y) {
    double sum = 0;
    for (int p = x - (k - 1); p <= x + (k - 1); p++)
        for (int q = y - (k - 1); q <= y + (k - 1); q++) {
            sum += gradientY(img, p, q) * gradientY(img, p, q);
        }
    return sum;
}

double GaborUtility::getAngle(IplImage *img, int x, int y) {
    double gxyVar = gxy(img, x, y);
    double gxxVar = gxx(img, x, y);
    double gyyVar = gyy(img, x, y);
    double angle = PI / 2 + .5 * atan((2 * gxyVar) / (gxxVar - gyyVar));

    if (isnan(angle)) angle = PI / 2 + .5 * atan(1);

    return angle;
}//*/

vector<double> GaborUtility::computePixel(IplImage *img, int x1, int y1) {
    vector<double> components;
    double x = (double) x1 / img->width;
    double y = (double) y1 / img->height;
    double theta = getAngle(img, x1, y1);
    double p1 = pow(e, -((pow(x, 2) + pow(y, 2)) / 18));
    double p2 = cos(.2 * PI * (x * cos(theta) + y * sin(theta)));
    double p3 = sin(.2 * PI * (x * cos(theta) + y * sin(theta)));

    components.push_back(p1 * p2); //real
    components.push_back(p1 * p3); //imaginary
    return components;
}

Mat_<double>** GaborUtility::computeFeatures(IplImage* img) {
    double ve;
    //Allocate return
    Mat_<double> **retn = new Mat_<double>*[img->width];
    for (int i = 0; i < img->width; i++) {
        retn[i] = new Mat_<double>[img->height];
    }


    IplImage * lab = cvCreateImage(cvGetSize(img), img->depth, img->nChannels);
    IplImage *aLayer = cvCreateImage(cvGetSize(img), img->depth, 1);
    IplImage *bLayer = cvCreateImage(cvGetSize(img), img->depth, 1);
    cvCvtColor(img, lab, CV_RGB2Lab);

    //extract a* and b* layers
    for (int i = 0; i < lab->imageSize; i += 3) {
        aLayer->imageData[i / 3] = lab->imageData[i + 1];
        bLayer->imageData[i / 3] = lab->imageData[i + 2];
    }

    Mat_<float> matA(aLayer->width, aLayer->height);
    Mat_<float> matB(bLayer->width, bLayer->height);
    for (int i = 0; i < aLayer->width; i++) {
        for (int j = 0; j < aLayer->height; j++) {
            ve = CV_IMAGE_ELEM(aLayer, uchar, j, i);
            matA(i, j) = (float) ve;
            ve = CV_IMAGE_ELEM(bLayer, uchar, j, i);
            matB(i, j) = (float) ve;
        }
    }
    double orientation;

    //get the energies for each pixel
    for (int i = 0; i < img->width; i++)
        for (int j = 0; j < img->height; j++) {
            Mat_<double> features(2, 1);

            orientation = getAngle(aLayer, i, j);
            double LEa = impulseReponse(i, j, orientation, matA);
            orientation = getAngle(bLayer, i, j);
            double LEb = impulseReponse(i, j, orientation, matB);

            features(0) = LEa;
            features(1) = LEb;

            retn[i][j] = features;
        }
    cvReleaseImage(&lab);
    cvReleaseImage(&aLayer);
    cvReleaseImage(&bLayer);
    return retn;

}

double GaborUtility::impulseReponse(int x, int y, double orientation, const Mat_<float>& src) {
    Mat_<float> real;
    Mat_<float> imag;
    long Width;
    creat_kernel(orientation, 1, 3, real, imag, Width);
    double r, i;

    r=conv(real, src, x, y, (Width - 1) / 2, (Width - 1) / 2);
    i=conv(imag, src, x, y, (Width - 1) / 2, (Width - 1) / 2);

    return sqrt(r * r + i * i);
}

//From cvgabor

void GaborUtility::creat_kernel(double orientation, double freq, double Sigma, Mat_<float>& Real, Mat_<float>& Imag, long& lWidth) {

    /**************************** Gabor Function ****************************/
    int x, y;
    double dReal;
    double dImag;
    double dTemp1, dTemp2, dTemp3;

    //determine the width of Mask
    double dModSigma = Sigma / freq;
    double dWidth = round(dModSigma * 6 + 1); //*2 instead *6
    //test whether dWidth is an odd.
    if (fmod(dWidth, 2.0) == 0.0) dWidth++;
    lWidth = (long) dWidth;

    Real.create(lWidth, lWidth);
    Imag.create(lWidth, lWidth);

    for (int i = 0; i < lWidth; i++) {
        for (int j = 0; j < lWidth; j++) {
            x = i - (lWidth - 1) / 2;
            y = j - (lWidth - 1) / 2;
            dTemp1 = (pow(freq, 2) / pow(Sigma, 2)) * exp(-(pow((double) x, 2) + pow((double) y, 2)) * pow(freq, 2) / (2 * pow(Sigma, 2)));
            dTemp2 = cos(freq * cos(orientation) * x + freq * sin(orientation) * y) - exp(-(pow(Sigma, 2) / 2));
            dTemp3 = sin(freq * cos(orientation) * x + freq * sin(orientation) * y);
            dReal = dTemp1*dTemp2;
            dImag = dTemp1*dTemp3;
            //gan_mat_set_el(pmReal, i, j, dReal);
            //cvmSet( (CvMat*)mReal, i, j, dReal );
            Real(i, j) = dReal;
            //gan_mat_set_el(pmImag, i, j, dImag);
            //cvmSet( (CvMat*)mImag, i, j, dImag );
            Imag(i, j) = dImag;

        }
    }
}

double GaborUtility::conv(const Mat_<float>& kernel, const Mat_<float>& src, int x, int y, int anchorX, int anchorY) {
    double result = 0;
    for (int i = 0; i < kernel.rows; ++i) {
        for (int j = 0; j < kernel.cols; ++j) {
            int ii = borderInterpolate(x + i - anchorX, src.rows, BORDER_DEFAULT);
            int jj = borderInterpolate(y + j - anchorY, src.cols, BORDER_DEFAULT);
            result += kernel(i, j) * src(ii, jj);
        }
    }
    return result;
}